#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

//#include<cv_bridge/cv_bridge.h>
#include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>

#include <map>

#include <pcl/filters/crop_box.h>

//apriltag
#include "livox_ros_driver/CustomMsg.h"
#include<deque>
#include<thread>
#include<mutex>
#include<sensor_msgs/PointCloud2.h>
#include <image_transport/image_transport.h>

using namespace Eigen;
using namespace std;
using PointType = pcl::PointXYZINormal;
ros::Publisher pc_pub_,pc_robosense_pub_,pc_robosense2veldyne_pub_;
image_transport::Publisher image_pub_;

// rslidar和velodyne的格式有微小的区别
// rslidar的点云格式
struct RsPointXYZIRT {
    PCL_ADD_POINT4D;
    uint8_t intensity;
    uint16_t ring = 0;
    double timestamp = 0;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(RsPointXYZIRT,
                                  (float, x, x)(float, y, y)(float, z, z)(uint8_t, intensity, intensity)
                                          (uint16_t, ring, ring)(double, timestamp, timestamp))

// velodyne的点云格式
struct VelodynePointXYZIRT {
    PCL_ADD_POINT4D

    PCL_ADD_INTENSITY;
    uint16_t ring;
    float time;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT,
                                   (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)
                                           (uint16_t, ring, ring)(float, time, time)
)

struct VelodynePointXYZIR {
    PCL_ADD_POINT4D

    PCL_ADD_INTENSITY;
    uint16_t ring;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIR,
                                   (float, x, x)(float, y, y)
                                           (float, z, z)(float, intensity, intensity)
                                           (uint16_t, ring, ring)
)

deque<sensor_msgs::PointCloud2Ptr> rs_pc_deq_;
mutex rs_pc_dep_mutex_;

void changeLivoxCustom2PC2(const livox_ros_driver::CustomMsg::ConstPtr &msg,pcl::PointCloud<PointType>::Ptr p_pc){
    PointType pt;
    for (uint i = 0; i < msg->point_num; i++)
    {
        if ((msg->points[i].line < 6) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))
        {
            pt.x = msg->points[i].x;
            pt.y = msg->points[i].y;
            pt.z = msg->points[i].z;
            pt.intensity = msg->points[i].reflectivity;
            pt.curvature = msg->points[i].offset_time; //use curvature as time of each laser points
            p_pc->points.push_back(pt);
        }
    }
}

void pc_custom_CB(const livox_ros_driver::CustomMsg::Ptr& pc_msg){
    pcl::PointCloud<PointType>::Ptr pc_tmp(new pcl::PointCloud<PointType>);//转成pcl点云
    changeLivoxCustom2PC2(pc_msg,pc_tmp);
    pc_tmp->width = 1;
    pc_tmp->height = pc_tmp->size();

    sensor_msgs::PointCloud2 tempCloud;
    pcl::toROSMsg(*pc_tmp, tempCloud);
    tempCloud.header = pc_msg->header;
    pc_pub_.publish(tempCloud);
}

void img_CB(const sensor_msgs::CompressedImageConstPtr& front_image_msg){
    cv_bridge::CvImagePtr image_front;
    image_front = cv_bridge::toCvCopy(front_image_msg, sensor_msgs::image_encodings::BGR8);
    
    sensor_msgs::ImagePtr msg = cv_bridge::CvImage(front_image_msg->header, "bgr8", (*image_front).image).toImageMsg();  //opencv 是按照bgr8显示的,不正确的话,图像显示有问题
    image_pub_.publish(msg);
}

template<typename T>
bool has_nan(const T& point) {

    // remove nan point, or the feature assocaion will crash, the surf point will containing nan points
    // pcl remove nan not work normally
    // ROS_ERROR("Containing nan point!");
    if (pcl_isnan(point.x) || pcl_isnan(point.y) || pcl_isnan(point.z)) {
        return true;
    } else {
        return false;
    }
}

template<typename T>
bool judgeLivoxRange(const T& pt){
    double ratio = sin(0.707);
    if(pt.y-ratio*pt.x -3< 0 && pt.y+ratio*pt.x +3>0 ){
        return true;
    }else{
        return false;
    }
}

void handle_pc_xyzin(const  pcl::PointCloud<RsPointXYZIRT>::Ptr &pc_in,
    const  pcl::PointCloud<pcl::PointXYZINormal>::Ptr &pc_out){
        pc_out->reserve(pc_in->size());
    // to new pointcloud
    for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) {
        if (has_nan(pc_in->points[point_id])||!judgeLivoxRange(pc_in->points[point_id])||pc_in->points[point_id].x<0)
            continue;
        pcl::PointXYZINormal new_point;
        new_point.x = pc_in->points[point_id].x;
        new_point.y = pc_in->points[point_id].y;
        new_point.z = pc_in->points[point_id].z;
        new_point.intensity = pc_in->points[point_id].intensity;

//        // 计算相对于第一个点的相对时间
       new_point.curvature = float((pc_in->points[point_id].timestamp - pc_in->points[0].timestamp)/(1e-9));
    //    cout<<"p time "<<to_string(pc_in->points[point_id].timestamp - pc_in->points[0].timestamp)<<" "<<
    //     to_string(new_point.curvature)<<endl;
        pc_out->points.emplace_back(new_point);
    }
}

void rsHandler_XYZIRT(const sensor_msgs::PointCloud2Ptr &pc_msg) {
    {
        lock_guard<mutex> lck(rs_pc_dep_mutex_);
        rs_pc_deq_.push_back(pc_msg);
    }
    pcl::PointCloud<RsPointXYZIRT>::Ptr pc_in(new pcl::PointCloud<RsPointXYZIRT>());
    pcl::fromROSMsg(*pc_msg, *pc_in);
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr pc_out(new pcl::PointCloud<pcl::PointXYZINormal>());
    handle_pc_xyzin(pc_in,pc_out);

    sensor_msgs::PointCloud2 pc_new_msg;
    pcl::toROSMsg(*pc_out, pc_new_msg);
    pc_new_msg.header = pc_msg->header;
    pc_robosense_pub_.publish(pc_new_msg);
}

void handle_pc_xyzin(const  pcl::PointCloud<RsPointXYZIRT>::Ptr &pc_in,
    const  pcl::PointCloud<VelodynePointXYZIRT>::Ptr &pc_out){
        pc_out->reserve(pc_in->size());
    // to new pointcloud
    for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) {
        if (has_nan(pc_in->points[point_id])||!judgeLivoxRange(pc_in->points[point_id])||pc_in->points[point_id].x<0)
            continue;
        VelodynePointXYZIRT new_point;
        new_point.x = pc_in->points[point_id].x;
        new_point.y = pc_in->points[point_id].y;
        new_point.z = pc_in->points[point_id].z;
        new_point.intensity = pc_in->points[point_id].intensity;

//        // 计算相对于第一个点的相对时间
       new_point.ring = pc_in->points[point_id].ring;
        new_point.time = float((pc_in->points[point_id].timestamp - pc_in->points[0].timestamp));
    //    cout<<"p time "<<to_string(pc_in->points[point_id].timestamp - pc_in->points[0].timestamp)<<" "<<
    //     to_string(new_point.curvature)<<endl;
        pc_out->points.emplace_back(new_point);
    }
}

void rs2velodyneThread(){
    while(true){
        pcl::PointCloud<RsPointXYZIRT>::Ptr pc_in(new pcl::PointCloud<RsPointXYZIRT>());
        sensor_msgs::PointCloud2 pc_new_msg;
        std_msgs::Header heapder_tmp;
        {
            lock_guard<mutex> lck(rs_pc_dep_mutex_);
            if(rs_pc_deq_.empty())
                continue;
            heapder_tmp = rs_pc_deq_.front()->header;
            pcl::fromROSMsg(*rs_pc_deq_.front(), *pc_in);
            rs_pc_deq_.pop_front();
        }
        pcl::PointCloud<VelodynePointXYZIRT>::Ptr pc_out(new pcl::PointCloud<VelodynePointXYZIRT>());
        handle_pc_xyzin(pc_in, pc_out);

        pcl::toROSMsg(*pc_out, pc_new_msg);
        pc_new_msg.header =heapder_tmp;
        pc_robosense2veldyne_pub_.publish(pc_new_msg);
        ros::Duration(0.00001).sleep();
    }
}

int main(int argc, char* argv[])
{
    // Initialize ROS
    ros::init(argc, argv, "read_bag");
    ros::NodeHandle nh;

    // ros::Subscriber pc_sub =  nh.subscribe("/livox/lidar", 20000, pc_custom_CB, ros::TransportHints().tcpNoDelay());
    ros::Subscriber img_sub =  nh.subscribe("/camera/color/image_raw/compressed", 20000, img_CB, ros::TransportHints().tcpNoDelay());

    // pc_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/livox/lidar_convert",10);
    image_transport::ImageTransport it(nh);
    image_pub_ = it.advertise("/camera/color/image_raw_convert", 10);

    ros::Subscriber subRobosensePC = nh.subscribe("/rslidar_points", 20000, rsHandler_XYZIRT, ros::TransportHints().tcpNoDelay());
    pc_robosense_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/rslidar_points_convert", 100);
    pc_robosense2veldyne_pub_= nh.advertise<sensor_msgs::PointCloud2>("/velodyne_points", 100);

    thread rs2vel_thread(rs2velodyneThread);
    rs2vel_thread.detach();

    cout<<endl<<"convert pointcloud launch!"<<endl;


    ros::MultiThreadedSpinner spinner(3);
    spinner.spin();
   
    return 0;
}
